import java.util.HashMap;
import java.awt.BorderLayout;
import java.awt.Dimension;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import javax.swing.JLabel;
import javax.swing.JTextPane;
import javax.swing.JButton;
import javax.swing.JFrame;
import javax.swing.JPanel;
import javax.swing.SwingUtilities;

import pcsr.hwVirtualization.*;

/**
 * @author Paulo Fagundes
 *
 */
public final class FollowWall extends JPanel implements IRobotObserver, ISensorObserver{
	
	private static final long serialVersionUID = -7479241377126913700L;

	private enum WallFollowed 
	{ 
		NONE, 
		Right, 
		Left
	};
	private enum PositionToWall 
	{
		TOO_CLOSE,
		JUST_RIGHT,
		TOO_FAR;
	}
	private IRobotObservable ctl = new Robot();
	private Actuator leftWheel;
	private Actuator rightWheel;

	private HashMap<String, Double> sensorValues = new HashMap<String, Double>();  //  @jve:decl-index=0:
	private String leftFrontSensor;
	private String leftBackSensor;
	private String rightFrontSensor;
	private String rightBackSensor;
		
	// Simulator
	private double TOO_CLOSE_THRESHOLD=0.5;
	private double TOO_FAR_THRESHOLD=0.2;
	// E-Puck
	//private double TOO_CLOSE_THRESHOLD=0.12;
	//private double TOO_FAR_THRESHOLD=0.1;
	private WallFollowed theCurrentWall = WallFollowed.NONE;  //  @jve:decl-index=0:
	
	private JButton btnStop = null;
	private JButton btnStart = null;
	private JLabel lbRobotName = null;
	private JTextPane tpSensorState = null;

	/**
	 * This method initializes 
	 * 
	 */
	public FollowWall() {
		super();
		initialize();
	}

	@Override
	public void RobotChanged(Robot robotStatus) {
		// TODO Auto-generated method stub

	}

	@Override
	public void SensorChanged(Sensor sensor) {
		String sensorID = sensor.GetSensorID();

		SwingUtilities.invokeLater(new Runnable(){

			@Override
			public void run() {
				UpdateSensorInfo();
			}});
		sensorValues.put(sensorID, sensor.GetSensorValue().GetValue());

		//WallFoolow();
		feedbackControl();
	}

	private void feedbackControl()
	{
		PositionToWall posWall = null;
		if(theCurrentWall == WallFollowed.NONE)
		{// Decide witch wall to follow
			chooseWallToFollow();
		}
		if(theCurrentWall == WallFollowed.Left) posWall = wallLeftDistanceProx();
		if(theCurrentWall == WallFollowed.Right) posWall = wallRightDistanceProx();		
				
		if(posWall == null) return;
		if(posWall == PositionToWall.TOO_CLOSE) veer_away_from_wall();
		if(posWall == PositionToWall.TOO_FAR) veer_toward_wall();
		if(posWall == PositionToWall.JUST_RIGHT) drive_straight();
	}
	
	private void WallFoolow()
	{
		// Simulator
//		double distanceNear = 0.5;
//		double distanceFar = 0.2;
		// E-Puck
		double distanceNear = 0.12;
		double distanceFar = 0.1;
		
		ActuatorValue rightWheelValue = null;
		ActuatorValue leftWheelValue = null;
		double leftF = Math.max(0, sensorValues.get(leftFrontSensor));
		double rightF = Math.max(0, sensorValues.get(rightFrontSensor));
		double leftB = Math.max(0, sensorValues.get(leftBackSensor));
		double rightB = Math.max(0, sensorValues.get(rightBackSensor));
		//System.out.println("["+leftF+"]["+leftB+"]["+rightF+"]["+rightB+"]");
		double leftTotal = leftB+leftF;
		double rightTotal = rightB+rightF;
		
		if( leftTotal >= rightTotal) 
		{// Robot is closer to the left
			if(leftB > distanceFar && leftB < distanceNear)
			{//sensor is in corridor
//				System.out.println("In Corridor ->"+leftB+"->"+distanceNear);
				rightWheelValue =  rightWheel.GetActuatorValue();
				rightWheelValue.SetValue(0.3);

				leftWheelValue =  leftWheel.GetActuatorValue();
				leftWheelValue.SetValue(0.3);
			}
			else
			{// sensor is not in corridor
				if( leftB > distanceNear)
				{//robot is too much near the the wall, turn right
//					System.out.println("distanceNear ->"+leftB+"->"+distanceNear);
					rightWheelValue =  rightWheel.GetActuatorValue();
					rightWheelValue.SetValue(0.2);

					leftWheelValue =  leftWheel.GetActuatorValue();
					leftWheelValue.SetValue(0.3);
				}
				else
				{
					if( leftB < distanceFar)
					{// robot is too far from the wall
//						System.out.println("distanceFar ->"+leftB+"->"+distanceNear);
						rightWheelValue =  rightWheel.GetActuatorValue();
						rightWheelValue.SetValue(0.4);
	
						leftWheelValue =  leftWheel.GetActuatorValue();
						leftWheelValue.SetValue(0.3);
					}
				}
			}
		}


		if(rightWheelValue != null)
		{
			rightWheel.SetActuatorValue(rightWheelValue);
		}
		if(leftWheelValue != null)
		{
			leftWheel.SetActuatorValue(leftWheelValue);
		}
	}
	
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		
		JFrame frame = new JFrame();
		frame.setTitle("Wall Following");
		frame.setSize(500, 120);
		frame.addWindowListener(new WindowAdapter() {
			public void windowClosing(WindowEvent e) {
				System.exit(0);
			}
		});

		java.awt.Container contentPane = frame.getContentPane();

		FollowWall ftw = new FollowWall();
		
		contentPane.add(ftw);

		frame.setVisible(true);
		//frame.pack();
	}
	
	private PositionToWall wallLeftDistanceProx()
	{
		double leftB = Math.max(0, sensorValues.get(leftBackSensor));
		
		if (leftB > TOO_CLOSE_THRESHOLD) return PositionToWall.TOO_CLOSE;
	    if (leftB < TOO_FAR_THRESHOLD) return PositionToWall.TOO_FAR;
	    return PositionToWall.JUST_RIGHT;
	}
	private PositionToWall wallRightDistanceProx()
	{
		double rightB = Math.max(0, sensorValues.get(rightBackSensor));
		
		if (rightB > TOO_CLOSE_THRESHOLD) return PositionToWall.TOO_CLOSE;
	    if (rightB < TOO_FAR_THRESHOLD) return PositionToWall.TOO_FAR;
	    return PositionToWall.JUST_RIGHT;
	}
	private void veer_away_from_wall()
	{
		ActuatorValue rightWheelValue = null;
		ActuatorValue leftWheelValue = null;
		rightWheelValue =  rightWheel.GetActuatorValue();
		leftWheelValue =  leftWheel.GetActuatorValue();
		if(theCurrentWall == WallFollowed.Left)
		{
			rightWheelValue.SetValue(0.2);
			leftWheelValue.SetValue(0.4);
		}
		if(theCurrentWall == WallFollowed.Right)
		{
			rightWheelValue.SetValue(0.4);
			leftWheelValue.SetValue(0.2);
		}
		if(rightWheelValue != null) rightWheel.SetActuatorValue(rightWheelValue);
		if(leftWheelValue != null) leftWheel.SetActuatorValue(leftWheelValue);
	}
	private void veer_toward_wall()
	{
		ActuatorValue rightWheelValue = null;
		ActuatorValue leftWheelValue = null;
		rightWheelValue =  rightWheel.GetActuatorValue();
		leftWheelValue =  leftWheel.GetActuatorValue();
		if(theCurrentWall == WallFollowed.Left)
		{
			rightWheelValue.SetValue(0.4);
			leftWheelValue.SetValue(0.3);
		}
		if(theCurrentWall == WallFollowed.Right)
		{
			rightWheelValue.SetValue(0.3);
			leftWheelValue.SetValue(0.4);
		}
		if(rightWheelValue != null) rightWheel.SetActuatorValue(rightWheelValue);
		if(leftWheelValue != null) leftWheel.SetActuatorValue(leftWheelValue);
	}
	private void drive_straight()
	{
		ActuatorValue rightWheelValue = null;
		ActuatorValue leftWheelValue = null;
		rightWheelValue =  rightWheel.GetActuatorValue();
		rightWheelValue.SetValue(0.3);
		leftWheelValue =  leftWheel.GetActuatorValue();
		leftWheelValue.SetValue(0.3);
		if(rightWheelValue != null)	rightWheel.SetActuatorValue(rightWheelValue);
		if(leftWheelValue != null) leftWheel.SetActuatorValue(leftWheelValue);
	}
	private void chooseWallToFollow() {
		double leftF = Math.max(0, sensorValues.get(leftFrontSensor));
		double rightF = Math.max(0, sensorValues.get(rightFrontSensor));
		double leftB = Math.max(0, sensorValues.get(leftBackSensor));
		double rightB = Math.max(0, sensorValues.get(rightBackSensor));
		double leftTotal = leftB+leftF;
		double rightTotal = rightB+rightF;
		if( leftTotal >= rightTotal) theCurrentWall = WallFollowed.Left;
		else theCurrentWall = WallFollowed.Right;		
	}
	
	private void Stop()
	{
		ActuatorValue rightWheelValue = rightWheel.GetActuatorValue();
		rightWheelValue.SetValue(0);

		ActuatorValue leftWheelValue = leftWheel.GetActuatorValue();
		leftWheelValue.SetValue(0);
		
		if(rightWheelValue != null)
		{
			rightWheel.SetActuatorValue(rightWheelValue);
		}
		if(leftWheelValue != null)
		{
			leftWheel.SetActuatorValue(leftWheelValue);
		}
	}
	
	private void Start()
	{
		ActuatorValue rightWheelValue = rightWheel.GetActuatorValue();
		rightWheelValue.SetValue(0.4);

		ActuatorValue leftWheelValue = leftWheel.GetActuatorValue();
		leftWheelValue.SetValue(0.4);
		
		if(rightWheelValue != null)
		{
			rightWheel.SetActuatorValue(rightWheelValue);
		}
		if(leftWheelValue != null)
		{
			leftWheel.SetActuatorValue(leftWheelValue);
		}
	}
	
	private void UpdateSensorInfo()
	{
		String sensorState = new String();
		
		sensorState += "Left Front : "+ sensorValues.get(leftFrontSensor)+"\n";
		sensorState += "Right Front: "+ sensorValues.get(rightFrontSensor)+"\n";
		sensorState += "Left Back  : "+ sensorValues.get(leftBackSensor)+"\n";
		sensorState += "Right Back : "+ sensorValues.get(rightBackSensor)+"\n";
		
		getTpSensorState().setText(sensorState);
	}
	
	/**
	 * This method initializes this
	 * 
	 */
	private void initialize() {
        this.setLayout(new BorderLayout());
        //this.setSize(new Dimension(500, 150));
        this.add(getLbName(), BorderLayout.NORTH);
        this.add(getBtnStart(),BorderLayout.WEST);
        this.add(getBtnStop(), BorderLayout.EAST);
        this.add(getTpSensorState(), BorderLayout.CENTER);
        
        ((Robot) ctl).Initialize();
		ctl.ObserveRobot(this);

		for(Actuator a : ctl.GetAllActuators())
		{
			if(a.GetActuatorID().toLowerCase().contains("left"))
			{
				leftWheel = a;
				ActuatorValue leftWheelValue =  leftWheel.GetActuatorValue();
				leftWheelValue.SetValue(0.6);
				leftWheel.SetActuatorValue(leftWheelValue);
			}
			if(a.GetActuatorID().toLowerCase().contains("right"))
			{
				rightWheel = a;
				ActuatorValue rightWheelValue =  rightWheel.GetActuatorValue();
				rightWheelValue.SetValue(0.6);
				rightWheel.SetActuatorValue(rightWheelValue);
			}
		}

		for(Sensor s : ctl.GetAllSensors())
		{
			if(s.GetSensorID().toLowerCase().contains("left"))
			{
				if(s.GetSensorID().toLowerCase().contains("front"))
				{
					leftFrontSensor = s.GetSensorID();
					sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
					s.ObserveSensor(this);
				}
				if(s.GetSensorID().toLowerCase().contains("back"))
				{
					leftBackSensor = s.GetSensorID();
					sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
					s.ObserveSensor(this);
				}
			}
			if(s.GetSensorID().toLowerCase().contains("right"))
			{
				if(s.GetSensorID().toLowerCase().contains("front"))
				{
					rightFrontSensor = s.GetSensorID();
					sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
					s.ObserveSensor(this);
				}
				if(s.GetSensorID().toLowerCase().contains("back"))
				{
					rightBackSensor = s.GetSensorID();
					sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
					s.ObserveSensor(this);
				}
			}	

			if(s.GetSensorID().toLowerCase().contains("1"))
			{
				rightFrontSensor = s.GetSensorID();
				sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
				s.ObserveSensor(this);
			}
			if(s.GetSensorID().toLowerCase().contains("2"))
			{
				rightBackSensor = s.GetSensorID();
				sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
				s.ObserveSensor(this);
			}
			if(s.GetSensorID().toLowerCase().contains("8"))
			{
				leftFrontSensor = s.GetSensorID();
				sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
				s.ObserveSensor(this);
			}
			if(s.GetSensorID().toLowerCase().contains("7"))
			{
				leftBackSensor = s.GetSensorID();
				sensorValues.put(s.GetSensorID(), s.GetSensorValue().GetValue());
				s.ObserveSensor(this);
			}
		}
		SwingUtilities.invokeLater(new Runnable(){

			@Override
			public void run() {
				getLbName().setText(((Robot) ctl).GetRobotInfo().GetName());
			}});
		
	}

	/**
	 * @return 
	 * 
	 */
	private JLabel getLbName() {
		if(lbRobotName == null)
		{
		lbRobotName = new JLabel();
        lbRobotName.setText("JLabel");
		}
		return lbRobotName;
	}

	/**
	 * This method initializes btnStop	
	 * 	
	 * @return java.swing.JButton	
	 */
	private JButton getBtnStop() {
		if (btnStop == null) {
			btnStop = new JButton();
			btnStop.setSize(new Dimension(100,25));
			btnStop.setText("Stop Robot");
			btnStop.setName("Stop");
			btnStop.addActionListener(new ActionListener(){

				@Override
				public void actionPerformed(ActionEvent e) {
					SwingUtilities.invokeLater(new Runnable(){

						@Override
						public void run() {
							Stop();
						}});
				}});
		}
		return btnStop;
	}

	/**
	 * This method initializes btnStart	
	 * 	
	 * @return javax.swing.JButton	
	 */
	private JButton getBtnStart() {
		if (btnStart == null) {
			btnStart = new JButton();
			btnStart.setSize(new Dimension(100,25));
			btnStart.setText("Start Robot");
			btnStart.setName("Start");
			btnStart.addActionListener(new ActionListener(){

				@Override
				public void actionPerformed(ActionEvent e) {
					SwingUtilities.invokeLater(new Runnable(){

						@Override
						public void run() {
							Start();
						}});
				}});
		}
		return btnStart;
	}

	/**
	 * This method initializes tpSensorState	
	 * 	
	 * @return javax.swing.JTextPane	
	 */
	private JTextPane getTpSensorState() {
		if (tpSensorState == null) {
			tpSensorState = new JTextPane();
		}
		return tpSensorState;
	}

}  //  @jve:decl-index=0:visual-constraint="10,10"
